import cv2
import numpy as np
# from picamera2 import Picamera2
import serial
import time
import math

# picam2 = Picamera2()
# picam2.exposure_mode = 'off'
# picam2.exposure_compensation = -2
# picam2.awb_mode = 'off'
# picam2.focus_mode = 'manual'
# picam2.focus_distance = 0.5
# picam2.start()
picam2 = cv2.VideoCapture(1)

ser = serial.Serial(port='/dev/ttyS0', baudrate=1152000)
ser.close()
ser.open()
num=0
n=0

# try:
while True:
    while True:
#     byte = ser.read()
#     if st == 0:
#         if byte == startChar:
#             st = 1
#     if st == 1:
#         if byte == stopChar:
#             st = 2
#         data += byte
#     if st == 2:
#         response = data
        #         ser.write(response)
        st = 0
#     if response:
    
        ret, frame = picam2.read()
#         frame = picam2.capture_array()
#         frame_bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
        blur = cv2.GaussianBlur(frame, (5, 5), 0)
        frameHSV = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
        lowerBound = np.array([0, 0, 255])
        upperBound = np.array([30, 30, 255])
        myMask = cv2.inRange(frameHSV, lowerBound, upperBound)
        xw = 0
        yh = 0
        w=0
        contours, junk = cv2.findContours(myMask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if len(contours) > 0:
            contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)
            contour = contours[0]
            x, y, w, h = cv2.boundingRect(contour)
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 3)
            xw = x + w // 2
            yh = cv2.contourArea(contour)
        num+=int(w)
#         print(num)
        n+=1
        if n==10:
            
            cv2.putText(frame, str(int(xw)) + ' x', (20,400),cv2.FONT_HERSHEY_SIMPLEX , 0.6, (0,0,255), 2)
            cv2.putText(frame, str(int(num//10)) + ' w', (80,400),cv2.FONT_HERSHEY_SIMPLEX , 0.6, (0,0,255), 2)
            num=0
            n=0
            print(xw)
            cv2.imshow("Camera Feed", myMask)
            cv2.imshow("Camera", frame)
            
            message = '@' + 'x' + str(xw) + '='
            ser.write(message.encode())
#             time.sleep(0.1)
#             message = '@' + 'y' + str(yh) + '='
#             ser.write(message.encode())
            message = '@' + 'd' + str(w) + '='
            ser.write(message.encode())
#-------------------------------------#
        t = 0
        firewater_data = f"{int(xw*100)},{int(yh*10)}\r\n"
        ser.write(firewater_data.encode())
        t+= 0.1
#         time.sleep(0.1)
    
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

# except KeyboardInterrupt:
#     pass
cv2.destroyAllWindows()
picam2.stop()
ser.close()
